/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#ifndef UNITREELEG_H
#define UNITREELEG_H

#include "common/mathTypes.h"
#include "common/enumClass.h"

class QuadrupedLeg{
public:
    /// @brief 创建四足机器人腿部实例, 存储腿部几何参数
    /// @param legID 序号:0~3
    /// @param abadLinkLength {thigh}与{hip}原点距离(沿旋转轴的偏移)
    /// @param hipLinkLength 大腿长(==thigh_length)
    /// @param kneeLinkLength 小腿长(==calf_length)
    /// @param pHip2B point hip to body, hip_link原点在{body}中的坐标
    QuadrupedLeg(int legID, float abadLinkLength, float hipLinkLength, 
                 float kneeLinkLength, Vec3 pHip2B);
    ~QuadrupedLeg(){}

    /// @brief 位置正运动学, 计算足端在{hip}系下的坐标
    /// @param q 三关节角度
    /// @return 足端在{hip}系下的坐标
    Vec3 calcPEe2H(Vec3 q);

    /// @brief 位置正运动学, 计算足端在{body}系下的坐标
    /// @param q 三关节角度
    /// @return 足端在{body}系下的坐标
    Vec3 calcPEe2B(Vec3 q);

    /// @brief 速度正运动学, 计算足端速度
    /// @param q 三关节角度
    /// @param qd 三关节速度
    /// @return 足端速度
    Vec3 calcVEe(Vec3 q, Vec3 qd);

    /// @brief 位置逆运动学, 根据足端位置计算关节角
    /// @param pEe 足端位置坐标
    /// @param frame 参考坐标系, 可选{hip},{body}
    /// @return 三关节角度
    Vec3 calcQ(Vec3 pEe, FrameType frame);

    /// @brief 速度逆运动学, 根据期望足端速度、当前关节角计算关节角速度
    /// @param q 当前关节角
    /// @param vEe 期望足端速度
    /// @return 关节角速度
    Vec3 calcQd(Vec3 q, Vec3 vEe);

    /// @brief 速度逆运动学, 根据{frame}下的当前足端位置和期望足端速度计算关节角速度
    /// @param pEe 当前足端位置
    /// @param vEe 期望足端速度
    /// @param frame 参考坐标系, 可选{hip},{body}
    /// @return 关节角速度
    Vec3 calcQd(Vec3 pEe, Vec3 vEe, FrameType frame);

    /// @brief 逆向静力学, 根据当前关节角和期望足端力计算关节力矩
    /// @param q 当前关节角
    /// @param force 期望足端力
    /// @return 关节力矩
    Vec3 calcTau(Vec3 q, Vec3 force);

    /// @brief 计算当前关节角下的jacob matrix
    /// @param q 当前关节角
    /// @return jacob matrix
    Mat3 calcJaco(Vec3 q);

    /// @brief 获取hip_link原点在{body}中的坐标
    /// @return 
    Vec3 getHip2B(){return _pHip2B;}

protected:
    float q1_ik(float py, float pz, float b2y);
    float q3_ik(float b3z, float b4z, float b);
    float q2_ik(float q1, float q3, float px, 
                float py, float pz, float b3z, float b4z);
    float _sideSign;
    const float _abadLinkLength;    //hip长
    const float _hipLinkLength;     //大腿长
    const float _kneeLinkLength;    //小腿长
    const Vec3 _pHip2B;     //hip_link原点在{body}中的坐标
};


class A1Leg : public QuadrupedLeg{
public:
    A1Leg(const int legID, const Vec3 pHip2B):
        QuadrupedLeg(legID, 0.0838, 0.2, 0.2, pHip2B){}
    ~A1Leg(){}
};


class Go1Leg : public QuadrupedLeg{
public:
    Go1Leg(const int legID, const Vec3 pHip2B):
        QuadrupedLeg(legID, 0.08, 0.213, 0.213, pHip2B){}
    ~Go1Leg(){}
};

#endif  // UNITREELEG_H